/******************************************************************
Copyright © LibKz 2017~2023. All rights reserved.
* @projectName   LibKz
* @author        Raiiwhen
* @email         masterraii@icloud.com
* @origin        \05_Console\01_WatchX\WatchX\ALGO
*******************************************************************/
#include "math.h"
#include "KalamnFilter.h"



void Kalmen_Filte_Init(	Filter_K* obj,
													float P00, float P01,
													float P10, float P11,
													float Q0, float Q1,
													float R,
													float dt
												){
		obj->dt = dt;
		obj->P[0][0] = P00;
		obj->P[0][1] = P01;
		obj->P[1][0] = P10;
		obj->P[1][1] = P11;
		obj->Q[0] = Q0;
		obj->Q[1] = Q1;
		obj->R = R;
}

/*Kalmen Filter*/
float raw_g, raw_a;
float Kalmen_Fusion(Filter_K* obj, float gyro, float angle){
	/*data container declare*/
	float _dt = obj->dt;
	float X[2] = {obj->X[0], obj->X[1]};
	float P[2][2] = {
		{obj->P[0][0], obj->P[0][1]},
		{obj->P[1][0], obj->P[1][1]}
	};
	float Q[2] = {obj->Q[0], obj->Q[1]};
	float R_Angle = obj->R;
	float K[2];
	/*raw for debug*/
	raw_g += gyro * _dt;
	if(raw_g > 90)	raw_g -= 180;
	if(raw_g < -90)	raw_g += 180;
	raw_a = angle;
	/*f1 Update X(k|k-1)*/
	X[0] += (gyro - X[1]) * _dt;
	X[1] = X[1];
	/*f2 Update P(k|k-1)*/
	P[0][0] = P[0][0] + Q[0]*_dt 	- P[0][1]*_dt - P[1][0]*_dt;   //a
	P[0][1] = P[0][1] 						- P[1][1]*_dt;    //b
	P[1][0] = P[1][0] 						- P[1][1]*_dt;    //c
	P[1][1] = P[1][1] + Q[1]*_dt;    //d
	/*f3 Update Kg(k)*/
	K[0] = P[0][0] / (R_Angle + P[0][0]);
	K[1] = P[1][0] / (R_Angle + P[0][0]);
	/*f4 Update X(k|k)*/
	X[0] = X[0] + K[0] * (angle - X[0]);
	X[1] = X[1] + K[1] * (angle - X[0]);
	/*f5 Update P(k|k)*/
	P[0][0] = P[0][0] - K[0] * P[0][0];
	P[0][1] = P[0][1] - K[0] * P[0][1];
	P[1][0] = P[1][0] - K[1] * P[0][0];
	P[1][1] = P[1][1] - K[1] * P[0][1];
	/*Update Struct*/
	obj->X[0] = X[0];
	obj->X[1] = X[1];
	obj->K[0] = K[0];
	obj->K[1] = K[1];
	obj->P[0][0] = P[0][0]; obj->P[0][1] = P[0][1];
	obj->P[1][0] = P[1][0]; obj->P[1][1] = P[1][1];
	
	return X[0];
}

#include "attitude.h"
#include "stm32f4xx.h"
#include "MPU.h"
#include "PinMap.h"
#include "MS5611.h"
#include "upload.h"
#include "math.h"
#include "KalamnFilter.h"
static vector3 Gacc, _Gyro, Gyro, Angl;
static vector3 Acc, Vel, Pos;
static vector3 bias_acc={
	.x=-0.08,
	.y=0.19,
	.z=9.7	
};
static vector3 bias_gyro= {
	.x=2.36,
	.y=0.55,
	.z=0.22
};
static float IMUTem;
#define SCALE 32768
#define ACC_K 0.00059821899414062 //+-2g@16-bit, acc(m*s^-2) = val * k, g = 9.80122m*s^-2(Beijing)
#define GYRO_K 0.0609793280078054 //+-2000dps@16-bit, gyro(dps) = val * k 
#define IMU_DT 0.001	//i.e. angle(deg) += gyro(dps) * dt(s)

void IMU_GetBias(vector3 g, vector3 a){
	static u16 IMU_CALI_CNT;
	static vector3 IMU_CALI_GYRO, IMU_CALI_ACC;
	static u16 ave_cnt = 8000;
	/*called by MPU Update*/
	IMU_CALI_CNT++;
	//callect 1000 samples to calculate
	IMU_CALI_GYRO.x += g.x;
	IMU_CALI_GYRO.y += g.y;
	IMU_CALI_GYRO.z += g.z;
	IMU_CALI_ACC.x += a.x;
	IMU_CALI_ACC.y += a.y;
	IMU_CALI_ACC.z += a.z;		
	/*finish one cali cycle, update bias*/
	if(IMU_CALI_CNT==ave_cnt){
		bias_gyro.x  = IMU_CALI_GYRO.x / ave_cnt;
		bias_gyro.y = IMU_CALI_GYRO.y / ave_cnt;
		bias_gyro.z = IMU_CALI_GYRO.z / ave_cnt;
		bias_acc.x = IMU_CALI_ACC.x / ave_cnt;
		bias_acc.y = IMU_CALI_ACC.y / ave_cnt;
		bias_acc.z = IMU_CALI_ACC.z / ave_cnt;
		/*init cali summary*/
		IMU_CALI_CNT=0;
		IMU_CALI_GYRO.x = 0;
		IMU_CALI_GYRO.y = 0;
		IMU_CALI_GYRO.z = 0;
		IMU_CALI_ACC.x = 0;
		IMU_CALI_ACC.y = 0;
		IMU_CALI_ACC.z = 0;	
		/*serial report result*/
		printf("%f | %f | %f | ",bias_gyro.x, bias_gyro.y, bias_gyro.z);
		printf("%f | %f | %f | ",bias_acc.x, bias_acc.y, bias_acc.z);
		printf("%f",get_IMUTem());
		printf("\r\n");
	}	
	/*serial and led*/
}

Filter_K Anglx_FK, Angly_FK;

void IMU_ALGO_Init(void){
	Kalmen_Filte_Init(&Anglx_FK,1,0,0,1,0.001, 0.003, 0.5, 0.001);
	Kalmen_Filte_Init(&Angly_FK,1,0,0,1,0.001, 0.003, 0.5, 0.001);
}

void IMU_Update(void){
	static u16 ATTITUDE_CNT;
	static float a_anglx, a_angly;
	short temp;
	u8 buff[15];
	/*called by MPU INT*/
	/*raw in SI*/
	MPU_Read_Len(0x00,0x3B,14,buff);
	temp = buff[0]<<8  | buff[1];		Acc.x = temp * ACC_K;
	temp = buff[2]<<8  | buff[3];		Acc.y = temp * ACC_K;
	temp = buff[4]<<8  | buff[5];		Acc.z = temp * ACC_K;
	temp = buff[6]<<8  | buff[7];		IMUTem = temp;
	temp = buff[8]<<8  | buff[9];		Gyro.x = temp * GYRO_K;
	temp = buff[10]<<8 | buff[11];	Gyro.y = temp * GYRO_K;
	temp = buff[12]<<8 | buff[13];	Gyro.z = temp * GYRO_K;
	buff[14] = '\0';
	//IMU_GetBias(Gyro, Acc);
	/*raw added bias*/
	Gyro.x = Gyro.x - bias_gyro.x;
	Gyro.y = Gyro.y - bias_gyro.y;
	Gyro.z = Gyro.z - bias_gyro.z;
	/*digital LPF*/
	/*1st differencial*/
	Gacc.x = Gyro.x-_Gyro.x;
	Gacc.y = Gyro.y-_Gyro.y;
	Gacc.z = Gyro.z-_Gyro.z;
	_Gyro = Gyro;
	/*1st integration*/
#ifdef KALMEN_EN
	a_anglx =  - atan(Acc.z/Acc.y) * 57.295779513082320876798154814105;
	if(a_anglx > 0)a_anglx -= 180;
	a_anglx += 90;
	a_angly =  - atan(Acc.z/Acc.x) * 57.295779513082320876798154814105;
	if(a_angly > 0)a_angly -= 180;
	a_angly += 90;
	a_angly = -a_angly;

	Angl.x = Kalmen_Fusion(&Anglx_FK, Gyro.x , a_anglx);
	Angl.y = Kalmen_Fusion(&Angly_FK, Gyro.y , a_angly);
	Angl.z += Gyro.z * IMU_DT;
	Angl.z = 0 ;
#else
	Angl.x += Gyro.x * IMU_DT;
	Angl.y += Gyro.y * IMU_DT;
	Angl.z += Gyro.z * IMU_DT;
#endif
	/*2nd integration*/
	Pos.x += Vel.x;
	Pos.y += Vel.y;
	Pos.z += Vel.z;
	/*serial and led*/
	ATTITUDE_CNT<999 ? (ATTITUDE_CNT++) : (ATTITUDE_CNT=0);
	ATTITUDE_CNT<100 ? LED_G=0 : (LED_G=1);
//	if(!(ATTITUDE_CNT%50)){
//		for(temp=0;temp<15;temp++)pu8(buff[temp]);
//	}
//	if(ATTITUDE_CNT==0){
//		printf("T.. ");
//		MS5611_TrigT();
//	}
//	if(ATTITUDE_CNT==999)printf("%5f\r\n",MS5611_ReadT());
	//	if(!ATTITUDE_CNT)printf("%6d,%6d,%6d",(short)Acc.x,(short)Acc.y,(short)Acc.z);
	//	if(!ATTITUDE_CNT)printf("%6d",(short)IMUTem);
	//	if(!ATTITUDE_CNT)printf("%6d,%6d,%6d\r\n",(short)Gyro.x,(short)Gyro.y,(short)Gyro.z);
	//	if(!ATTITUDE_CNT)printf("\r\n");//127ms
}


/*SINS transfrom*/
void Rotation_Update(void){
	
}
void Frame_B2E(vector3 B, vector3* E){
	
}
void Frame_E2B(vector3 E, vector3* B){
	
}

/*extern API*/
float get_accx(void)	{	return Acc.x*ACC_K;}
float get_accy(void)	{	return Acc.y*ACC_K;}
float get_accz(void)	{	return Acc.z*ACC_K;}
float get_velx(void)	{	return Vel.x*ACC_K*IMU_DT;}
float get_vely(void)	{	return Vel.y*ACC_K*IMU_DT;}
float get_velz(void)	{	return Vel.z*ACC_K*IMU_DT;}
float get_posx(void)	{	return Pos.x*ACC_K*IMU_DT;}
float get_posy(void)	{	return Pos.y*ACC_K*IMU_DT;}
float get_posz(void)	{	return Pos.z*ACC_K*IMU_DT;}
float get_gaccx(void)	{	return Gacc.x*GYRO_K;}
float get_gaccy(void)	{	return Gacc.y*GYRO_K;}
float get_gaccz(void)	{	return Gacc.z*GYRO_K;}

float get_gyrox(void)	{	return Gyro.x;}
float get_gyroy(void)	{	return Gyro.y;}
float get_gyroz(void)	{	return Gyro.z;}
float get_anglx(void)	{	return Angl.x;}
float get_angly(void)	{	return Angl.y;}
float get_anglz(void)	{	return Angl.z;}
float get_IMUTem(void){	return IMUTem/100;}

void rst_anglx(void){Angl.x=0;}
void rst_angly(void){Angl.y=0;}
void rst_anglz(void){Angl.z=0;}
